/**
 * \file
 *
 * \brief Dronus is a extension for Multiwii, based on Atmel SAM3 Cortex-M3 MCU (ARM/32 bits) and develope on AtmelStudio 6.2.
 *
 */

#include <asf.h>

#include <stdio.h>
#include <due.h>
#include <rtos.h>
#include <dronus.h>
#include <led.h>
#include <console.h>
#include <flash.h>
#include <cli.h>
#include <pulses.h>
#include <pwms.h>

pulses_t pulses;

void DRONUS_setup( void );
static void main_task ( void *pvParameters );

void DRONUS_setup( void )
{
	vTraceSetISRProperties(TC_IRQn, "TC ISR", PULSE_IRQ_PRIORITY);
	vTraceSetISRProperties(ADC_IRQn, "ADC ISR", PWM_IRQ_PRIORITY);
	vTraceSetISRProperties(UOTGHS_IRQn, "MSC ISR", MSC_IRQ_PRIORITY);
	vTraceSetISRProperties(UART_IRQn, "UART ISR", USART_IRQ_PRIORITY);	
	vTraceSetISRProperties(USART0_IRQn, "USART0 ISR", USART_IRQ_PRIORITY);
	vTraceSetISRProperties(USART1_IRQn, "USART1 ISR", USART_IRQ_PRIORITY);
	vTraceSetISRProperties(USART2_IRQn, "USART2 ISR", USART_IRQ_PRIORITY);
	vTraceSetISRProperties(USART3_IRQn, "USART3 ISR", USART_IRQ_PRIORITY);
	vTraceSetISRProperties(TWI0_IRQn, "TWI0 ISR", TWI_IRQ_PRIORITY);
	vTraceSetISRProperties(TWI1_IRQn, "TWI1 ISR", TWI_IRQ_PRIORITY);
	
	usb_access=xSemaphoreCreateMutex();
	vTraceSetMutexName(usb_access, "USB");
	
	xTaskCreate(main_task, "MAIN" , MAIN_TASK_STACK_SIZE, NULL, MAIN_TASK_PRIORITY,	NULL);
}

static void main_task ( void *pvParameters )
{
	portTickType xNextWakeTime;
	char buffer[128];
	
	sprintf(buffer, STRING_HEADER);
	CONSOLE_write(buffer);
	
	xSemaphoreHandle semaphore_pulses=xSemaphoreCreateBinary();
	
	PWM_conf(5, PWM_FREQUENCY_LOW, 2);
	
	xNextWakeTime = xTaskGetTickCount();
	while (1)
	{
		PULSES_get_pulses(&pulses, PULSES_MAX_BLOCK_TIME, semaphore_pulses);
		xSemaphoreTake(semaphore_pulses, PULSES_MAX_BLOCK_TIME);
		sprintf(buffer, "ULTRASONIDO: up:%d us - down:%d us - freq:%d hz - distance:%d cm\r\n", (int) pulses[0].up, (int)  pulses[0].down, (int)  (1000000/( pulses[0].up + pulses[0].down)), (int) (pulses[0].up/58));
		CONSOLE_write(buffer);
		
		vTaskDelayUntil( &xNextWakeTime, MAIN_ELAPSE );
	}
}

int main (void)
{
	DUE_setup();
	RTOS_setup();
	CONSOLE_setup();
	LED_setup();
	FLASH_setup();
	CLI_setup();
	PULSES_setup();
	PWM_setup();
	
	DRONUS_setup();
	RTOS_start();
}
